package com.rockwell.sniffyhunter.model;

import java.util.Map;

import org.opencv.core.Point;

import com.rockwell.sniffyhunter.utils.DetectionIndex;
import com.rockwell.sniffyhunter.utils.IDetector;
import com.rockwell.sniffyhunter.utils.IRobotPositionDetector;
import com.rockwell.sniffyhunter.utils.IRobotSensors;
import com.rockwell.sniffyhunter.utils.IStateMachine;
import com.rockwell.sniffyhunter.utils.MoveToNextZoneState;
import com.rockwell.sniffyhunter.utils.UnloadMazeLocatorState;

/**
 * This class provides the robot for locating the maze
 * The following actions will be performed:
 * <ul>
 * <li>Jog around and sniff.
 * <li>Look out for maze (difficult type).
 * </ul>
 * <p>
 * 
 * @author      Sniffy Software Team
 * @version     1.0
 */

public class UnloadMazeLocator extends StateObject {
	
	private IRobotSensors m_sensors;
	private RobotCommandSequencer m_commandSequencer;
	private RobotPosition m_robotPosition;
	private IRobotPositionDetector m_positionDetector;
	private Map<Integer, IDetector> m_colorDetectors;
	private ObstacleSkipper m_obstacleSkipper;
	private boolean jogStarted;
	private static final int SCAN_ROW_WIDTH = 150; //mm
	private static final int REVERSE_DISTANCE = 50; //mm
	private final static double LOWER_ROTATE_ANGLE = 2;
	private final static double UPPER_ROTATE_ANGLE = 5;
	private final static double LOWER_INDEX_DISTANCE = 5;
	private final static double UPPER_INDEX_DISTANCE = 20;
	private double m_rotateAngle;
	private double indexDistance;
	private boolean m_turnRight;
	private int m_mazeDetected;
	public Picker m_picker;

	public UnloadMazeLocator(Robot robot) {
		super(robot);
		m_commandSequencer = new RobotCommandSequencer(robot);
		m_obstacleSkipper = new ObstacleSkipper(robot);
		m_picker = new Picker(robot);
		m_positionDetector = robot.m_positionDetector;
		m_colorDetectors = robot.m_colorDetectors;
		m_sensors = robot.m_sensors;
	}

	@Override
	public void Initialize() {
		//m_commandSequencer.Initialize();
		//m_obstacleSkipper.Initialize();
		//m_picker.Initialize();
		machineState = UnloadMazeLocatorState.START;
		machineRecoveryState = machineState;
		jogStarted = false;
		m_turnRight = false;
		m_mazeDetected = DetectionIndex.MAZE1;
	}
	
	public int getMazeDetected() {
		return m_mazeDetected;
	}
	
	@Override
	public boolean IsDone() {
		return machineState == UnloadMazeLocatorState.DONE;
	}

	public IStateMachine scan() {

		m_robotPosition = m_positionDetector.getPosition();

		if (m_colorDetectors.get(DetectionIndex.TREASURE).isPresent()) {
			// Try to pick the treasure
			machineRecoveryState = UnloadMazeLocatorState.JOG_STOPPED_TO_PICK_TREASURE;
			machineState = UnloadMazeLocatorState.STOP_JOG;
		}
		else if (anyMazeDectected()) {
			//gotcha!
			machineRecoveryState = UnloadMazeLocatorState.CHECK_MAZE_POS;
			machineState = UnloadMazeLocatorState.STOP_JOG;
		}
		else if (!m_sensors.isWheelMoving()) {
			if (m_robotPosition.frontwalldetected) {
				log("frontwalldetected = true.UnloadMazeLocator", 4);
				if ((m_turnRight && m_robotPosition.rightwalldetected) ||
					(!m_turnRight && m_robotPosition.leftwalldetected)) {
					//encounter a corner wall
					m_turnRight = !m_turnRight;
					machineState = UnloadMazeLocatorState.REVERSE_DUE_TO_CORNER_WALL;
				}
				else {
					//encounter a play area wall
					machineState = UnloadMazeLocatorState.REVERSE_DUE_TO_PLAY_WALL;
				}
			}
			else {
				log("frontwalldetected = false.UnloadMazeLocator", 4);
				machineState = UnloadMazeLocatorState.JOG;
			}
		}
		
		return machineState;
	}
	
	public IStateMachine reverseDueToCornerWall() {
		
		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.MoveBackward, REVERSE_DISTANCE);
		
		if (returnVal == ReturnResult.Completed) {
			log("reverseDueToCornerWall.UnloadMazeLocator", 4);
			
			machineState = UnloadMazeLocatorState.TURN_DUE_TO_CORNER_PLAY_WALL_DETECTED_BY_FRONT_SIDE_SENSORS;
		}
		
		return machineState;
	}
	
	public IStateMachine jog() {
		
		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.Jog, 3);
		
		if (returnVal == ReturnResult.Completed) {
			log("jog.UnloadMazeLocator", 4);
			
			jogStarted = true;
			machineState = UnloadMazeLocatorState.SCAN;
		}
		
		return machineState;
	}
	
	public IStateMachine stopJog() {
		
		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.Stop);
		
		if (returnVal == ReturnResult.Completed) {
			log("stopJog.UnloadMazeLocator", 4);
			
			jogStarted = false;
			machineState = machineRecoveryState;
		}
		
		return machineState;
	}
	
	public IStateMachine navAwayFromObs() {

		ReturnResult returnVal = m_obstacleSkipper.PerformAction();

		if (returnVal == ReturnResult.Completed) {
			log("navAwayFromObs.UnloadMazeLocator", 4);
			machineState = UnloadMazeLocatorState.SCAN;
		}
		
		return machineState;
	}
	
	public IStateMachine reverseDueToPlayWall() {
		
		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.MoveBackward, REVERSE_DISTANCE);
		
		if (returnVal == ReturnResult.Completed) {
			log("reverseDueToPlayWall.UnloadMazeLocator", 4);
			
			machineState = UnloadMazeLocatorState.TURN_DUE_TO_PLAY_WALL;
		}
		
		return machineState;
	}

	public IStateMachine turnDueToPlayWall() {
		
		ReturnResult returnVal = m_commandSequencer.PerformAction(m_turnRight? RobotAction.TurnRight : RobotAction.TurnLeft);
		
		if (returnVal == ReturnResult.Completed) {
			log("turnRightEncounterPlayWall.UnloadMazeLocator", 4);
			
			machineState = UnloadMazeLocatorState.SCAN_AFTER_TURN_DUE_TO_PLAY_WALL;
		}
		
		return machineState;
	}
	
	public IStateMachine scanAfterTurnDueToPlayWall() {

		m_robotPosition = m_positionDetector.getPosition();
		
		if (m_robotPosition.frontwalldetected) {
			log("frontwalldetected = true.UnloadMazeLocator", 4);

			if (anyMazeDectected()) {
				//gotcha!
				machineState = UnloadMazeLocatorState.DONE;
			}
			else {
				machineState = UnloadMazeLocatorState.TURN_DUE_TO_CORNER_PLAY_WALL;
			}
		}
		else {
			log("frontwalldetected = false.UnloadMazeLocator", 4);
			machineState = UnloadMazeLocatorState.MOVE_TO_NEXT_SCAN_ROW;
		}
		
		return machineState;
	}
	
	public IStateMachine moveToNextScanRow() {
		
		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.MoveForward2, SCAN_ROW_WIDTH);
		
		if (returnVal == ReturnResult.Completed) {
			log("moveToNextScanRow.UnloadMazeLocator", 4);
			
			machineState = UnloadMazeLocatorState.SCAN_AFTER_MOVE_TO_NEXT_SCAN_ROW;
		}
		
		return machineState;
	}
	
	public IStateMachine scanAfterMoveToNextScanRow() {

		m_robotPosition = m_positionDetector.getPosition();
		
		if (m_robotPosition.frontwalldetected) {
			log("frontwalldetected = true.UnloadMazeLocator", 4);

			if (anyMazeDectected()) {
				//gotcha!
				machineState = UnloadMazeLocatorState.DONE;
			}
			else {
				machineState = UnloadMazeLocatorState.TURN_DUE_TO_CORNER_PLAY_WALL;
			}
		}
		else {
			log("frontwalldetected = false.UnloadMazeLocator", 4);
			machineState = UnloadMazeLocatorState.TURN_AFTER_MOVE_TO_NEXT_SCAN_ROW;
		}
		
		return machineState;
	}
	
	public IStateMachine turnAfterMoveToNextScanRow() {
		
		ReturnResult returnVal = m_commandSequencer.PerformAction(m_turnRight? RobotAction.TurnRight : RobotAction.TurnLeft);
		
		if (returnVal == ReturnResult.Completed) {
			log("turnAfterMoveToNextScanRow.UnloadMazeLocator", 4);
			
			m_turnRight = !m_turnRight;
			machineState = UnloadMazeLocatorState.SCAN;
		}
		
		return machineState;
	}
	
	public IStateMachine turnDueToCornerPlayWall() {
		
		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.TurnBack);
		
		if (returnVal == ReturnResult.Completed) {
			log("turnDueToCornerPlayWall.UnloadMazeLocator", 4);
			
			m_turnRight = !m_turnRight;
			machineState = UnloadMazeLocatorState.SCAN;
		}
		
		return machineState;
	}
	
	public IStateMachine turnDueToCornerPlayWallDetectedByFrontAndSideSensors() {
		
		ReturnResult returnVal = m_commandSequencer.PerformAction(m_turnRight? RobotAction.TurnRight : RobotAction.TurnLeft);
		
		if (returnVal == ReturnResult.Completed) {
			log("turnDueToCornerPlayWallDetectedByFrontAndSideSensors.UnloadMazeLocator", 4);
			
			machineState = UnloadMazeLocatorState.SCAN;
		}
		
		return machineState;
	}
	
	public IStateMachine CheckMazePos() {

		if (!m_colorDetectors.get(m_mazeDetected).isPresent()) {
			log("MAZE_LOST.UnloadMazeLocator", 1);
			machineState = UnloadMazeLocatorState.SCAN;
		}
		else {
			Point mazeCenterDrift = m_colorDetectors.get(m_mazeDetected).getDrift();
			m_robotPosition = m_positionDetector.getPosition();
			
			if ((mazeCenterDrift.x == 0 && mazeCenterDrift.y == 0) || m_robotPosition.frontwalldetected) {
				//Maze at screen center, destination reached.
				log("DESTINATION_REACHED.UnloadMazeLocator", 1);
				machineState = UnloadMazeLocatorState.DONE;
			}
			else if (m_colorDetectors.get(DetectionIndex.TREASURE).isPresent()) {
				// Try to pick the treasure
				machineRecoveryState = UnloadMazeLocatorState.CHECK_MAZE_POS;
				machineState = UnloadMazeLocatorState.PICK_TREASURE;
			}
			else {
				if (mazeCenterDrift.y != 0) {
					//Maze not in the screen center, rotate to center
					String msg = String.format("rotate to center[Y.1f].UnloadMazeLocator", mazeCenterDrift.y);
					log(msg, 1);
					if (mazeCenterDrift.y > 0) {
						if (mazeCenterDrift.y > 0.15f) {
							m_rotateAngle = UPPER_ROTATE_ANGLE;
						}
						else {
							m_rotateAngle = LOWER_ROTATE_ANGLE;
						}
					}
					else {
						if (mazeCenterDrift.y < -0.15f) {
							m_rotateAngle = -UPPER_ROTATE_ANGLE;
						}
						else {
							m_rotateAngle = -LOWER_ROTATE_ANGLE;
						}
					}
					machineState = UnloadMazeLocatorState.ROTATE_TO_CENTER_MAZE;
				}
				else {
					//Maze in the screen center, index closer
					String msg = String.format("index forward[X%.1f].UnloadMazeLocator", mazeCenterDrift.x);
					log(msg, 1);
					if (mazeCenterDrift.x > 0) {
						if (mazeCenterDrift.x > 0.15f) {
							indexDistance = -UPPER_INDEX_DISTANCE;
						}
						else {
							indexDistance = -LOWER_INDEX_DISTANCE;
						}
					}
					else {
						if (mazeCenterDrift.x < -0.15f) {
							indexDistance = UPPER_INDEX_DISTANCE;
						}
						else {
							indexDistance = LOWER_INDEX_DISTANCE;
						}
					}
					machineState = UnloadMazeLocatorState.INDEX_TO_CENTER_MAZE;
				}
			}
		}

		return machineState;
	}
	
	public IStateMachine rotateToCenterMaze() {
		
		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.TurnDegree, m_rotateAngle);
		
		if (returnVal == ReturnResult.Completed) {
			log("rotateToCenterMaze.UnloadMazeLocator", 4);
			
			if (m_colorDetectors.get(DetectionIndex.TREASURE).isPresent()) {
				machineRecoveryState = UnloadMazeLocatorState.SCAN;
				machineState = UnloadMazeLocatorState.PICK_TREASURE;
			}
			else {
				machineState = UnloadMazeLocatorState.CHECK_MAZE_POS;
			}
		}
		
		return machineState;
	}
	
	public IStateMachine indexToCenterMaze() {
		
		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.Move2, indexDistance);
		
		if (returnVal == ReturnResult.Completed) {
			log("indexToCenterMaze.UnloadMazeLocator", 4);
			
			if (m_colorDetectors.get(DetectionIndex.TREASURE).isPresent()) {
				machineRecoveryState = UnloadMazeLocatorState.SCAN;
				machineState = UnloadMazeLocatorState.PICK_TREASURE;
			}
			else {
				machineState = UnloadMazeLocatorState.CHECK_MAZE_POS;
			}
		}
		
		return machineState;
	}
	
	public IStateMachine jogStoppedToPickTreasure() {
		machineRecoveryState = UnloadMazeLocatorState.SCAN;
		machineState = UnloadMazeLocatorState.PICK_TREASURE;
		return machineState;
	}
	
	public IStateMachine pickTreasure() {
		ReturnResult returnVal =  m_picker.PerformAction();
		if (returnVal == ReturnResult.Completed/*Pick Done*/) {
			machineState = machineRecoveryState;
		}
		
		return machineState;
	}
	
	public boolean anyMazeDectected() {
		boolean isMaze1Present = m_colorDetectors.get(DetectionIndex.MAZE1).isPresent();
		boolean isMaze2Present = m_colorDetectors.get(DetectionIndex.MAZE2).isPresent();
		boolean isMaze3Present = m_colorDetectors.get(DetectionIndex.MAZE3).isPresent();
		
		double mazeXValue = -1;
		if (isMaze3Present) {
			double maze3XValue = m_colorDetectors.get(DetectionIndex.MAZE3).getCenter().x;
			mazeXValue = maze3XValue;
			m_mazeDetected = DetectionIndex.MAZE3;
		}

		if (isMaze2Present) {
			double maze2XValue = m_colorDetectors.get(DetectionIndex.MAZE2).getCenter().x;
			if (mazeXValue < maze2XValue) {
				mazeXValue = maze2XValue;
				m_mazeDetected = DetectionIndex.MAZE2;
			}
		}
		
		if (isMaze1Present) {
			double maze1XValue = m_colorDetectors.get(DetectionIndex.MAZE1).getCenter().x;
			if (mazeXValue < maze1XValue) {
				mazeXValue = maze1XValue;
				m_mazeDetected = DetectionIndex.MAZE1;
			}
		}
		
		return (isMaze1Present || isMaze2Present || isMaze3Present);
	}
}